clc
clear all
%%机器人模型导入
%urdf模型导入
robot = importrobot("C:\Users\Admin\Desktop\biped_v2\xacro\leg_v2_serial_left.urdf");
% robot = importrobot("C:\Users\Admin\Desktop\biped_v2\xacro\leg_v2_serial_left1.urdf");
robot.DataFormat='column';
robot.Gravity =[0 0 -9.81];%%



%%根据urdf模型确定刚体系统自由度
N_fixed = 0;
for i = 1:1:robot.NumBodies
    if( strcmp(robot.Bodies{1, i}.Joint.Type,'fixed') == true)
        N_fixed=N_fixed+1;
    end   
end
N_dof = robot.NumBodies - N_fixed;

%创建机器人刚体系统参数结构体：robot_para
robot_para.JointAxis = zeros(3,N_dof);
robot_para.JointToParentTransform = zeros(4,4,N_dof);
for i = 1:1:N_dof
    robot_para.JointAxis(:,i) = robot.Bodies{1,i+1}.Joint.JointAxis;
    robot_para.JointToParentTransform(:,:,i) = robot.Bodies{1,i+1}.Joint.JointToParentTransform;
end

%定义连杆的惯量 N_dof个自由度的连杆
I_c=zeros(3,3,N_dof);
I_o=zeros(3,3,N_dof);
d_oc=zeros(3,N_dof);
m=zeros(1,N_dof); %质量

%根据所导入的urdf模型读取各连杆的质量 惯量（相对于各刚体坐标系原点） 和质心位置向量（相对于各个刚体坐标系原点）
for i = 1:1:N_dof
    m(i) = robot.Bodies{1, i+1}.Mass; %由于urdf模型中，第一个刚体为固定基，因此第一个活动连杆的索引为2， 也是循环i+1的由来
    d_oc(:,i) = robot.Bodies{1, i+1}.CenterOfMass';
end

%模型得到的动力学参数集合 param =[mi micx micy micz i_xx i_xy i_xz i_yy i_yz i_zz]
param_model_matrix=zeros(10,N_dof);
for i=1:N_dof
    param_model_matrix(1,i)=m(i);
    param_model_matrix(2:4,i)=m(i)*d_oc(:,i);
    %%从urdf中导入各连杆相对于对应关节坐标系的转动惯量
    param_model_matrix(5,i)=robot.Bodies{1,i+1}.Inertia(1);% I_xx
    param_model_matrix(6,i)=robot.Bodies{1,i+1}.Inertia(6);% I_xy
    param_model_matrix(7,i)=robot.Bodies{1,i+1}.Inertia(5);% I_xz
    param_model_matrix(8,i)=robot.Bodies{1,i+1}.Inertia(2);% I_yy
    param_model_matrix(9,i)=robot.Bodies{1,i+1}.Inertia(4);% I_yz
    param_model_matrix(10,i)=robot.Bodies{1,i+1}.Inertia(3);% I_zz
end
Para_vec=zeros(N_dof*10,1);
for i = 1:1:N_dof
    Para_vec(10*(i-1)+1:10*i)=param_model_matrix(:,i);
end


%%数据读取
% data = xlsread('C:\Users\Admin\Desktop\data0926\2023-09-26.12时18分25秒_log.csv',1,'A1:O19999')
data1 = readtable('C:\Users\Admin\Desktop\Dynamic_ParaEstimation\2023-09-26.12时18分25秒_log.csv');
data = table2array(data1);
data_q = data(:,1:5)';
data_qd = data(:,6:10)';
data_tau = data(:,11:15)';

t = 0.002;%采样周期 0.002s
N_data =size(data_q,2);
data_qdd = zeros(5,N_data - 1);

for i = 1:N_data -1
    data_qdd(:,i) = (data_qd(:,i+1) - data_qd(:,i))/t; 
end
K = zeros(N_dof,N_dof*10,N_data - 1);
K_aug = zeros(N_dof,N_dof*12,N_data - 1);
for i = 1: N_data - 1
    [U,Uj] = Compute_LinearDynmatrix(data_q(:,i), data_qd(:,i), data_qdd(:,i),robot_para);
    K(:,:,i) = Uj;
    K_aug(:,1:N_dof*10,i)=Uj;
    for j = 1:N_dof
        K_aug(j,N_dof*10 + (j-1)*2 + 1,i) = sign(data_qd(j,i));  %Bc*sign(joint_velocity)
        K_aug(j,N_dof*10 + (j-1)*2 + 2,i) = data_qd(j,i);  % B*joint_velocity
    end
end
%%数据预处理及线性动力学矩阵计算完成
%%*****************************************%%
%                                           %
%%*****************************************%%

%%开始进行优化建模及求解
nb=size(data_tau,1);
n_data = 5000; %小于19998的数

cvx_begin sdp
    cvx_solver Mosek
    cvx_precision high
    variable X(nb*10)
%     variable X_aug(nb*12)

    variable t1
    variable Bc(nb) %静摩擦力项
    variable B(nb)  %速度相关的粘滞摩擦力项
    expressions inertial_sdp_matrix(4,4,nb) % semidefinite  symmetric
    expressions C1(4,4) 
    expressions C2(4,4) 
    expressions C3(4,4) 
    expressions mj(nb)
    expressions hj(3,1,nb)
    expressions Ij(3,3,nb)
    expression error_norm1(n_data,1)
    
    joint_vel = data_qd;
    tau = data_tau;
    X_ref = Para_vec;
    beta = 0.001;
    inertia_normal_vec = zeros(10*nb,1);
    normal_vec = zeros(10,1);
    normal_vec = [1 20 20 20 100 100 100 100 100 100]';
    inertia_normal_vec = [normal_vec;
                            normal_vec;
                            normal_vec;
                            normal_vec;
                            normal_vec];
    

    K_square = zeros(size(K,2),size(K,2),n_data);
    K_sum =  zeros(size(K,2),size(K,2));
    BK_sum = zeros(1,size(K,2));
    tau_square_sum =0;
    for i=1:n_data
        %%二次项累加系数
        K_square(:,:,i) = K(:,:,i)'*K(:,:,i);
        K_sum = K_sum +  K_square(:,:,i);
        %%常数项平方系数 sum(tau.T*K)=b.T*G
        BK_sum =  BK_sum +tau(:,i)'* K(:,:,i);
        tau_square_sum = tau_square_sum + tau(:,i)'*tau(:,i);
    end
    K_sum = K_sum;% + 0.0001*eye(50); %%为了G能够求逆矩阵
    [Vk,Dk] = eig(K_sum);
    G = Vk*sqrt(Dk)*inv(Vk);
    G = real(G);  %% sum(K) = K_sum = G*G = G.transpose *G
%     G_inv = pinv(G);

    %% sum(tau.T*K)=b.T*G ;  b.T = sum(tau.T*K)*G^-1
    b = -BK_sum*pinv(G);
    b=b';
    %%从而求得||GX+b||^2 -bT*b + sum(tau.T*tau) 中的二次型系数G 和 b
    %%因此，将目标函数 sum(||YX-tau||^2) 转化为 ||GX+b||^2 -bT*b + sum(tau.T*tau)
    
    
    
    for i=1:n_data
%         error_norm1(i) = norm(K(:,:,i)*X-tau(:,i),2);
%          error_norm1(i) = norm(K(:,:,i)*X + Bc.*sign(joint_vel(:,i)) + B.*joint_vel(:,i)-tau(:,i),2); 
         error_norm1(i) = (K(:,:,i)*X + Bc.*sign(joint_vel(:,i)) + B.*joint_vel(:,i)-tau(:,i))'*(K(:,:,i)*X + Bc.*sign(joint_vel(:,i)) + B.*joint_vel(:,i)-tau(:,i));    
%          error_norm1(i) = (K(:,:,i)*X -tau(:,i))'*(K(:,:,i)*X-tau(:,i));
    end
%     error_norm1 = error_norm1/n_data;
%     minimize sum(error_norm1)/n_data + beta*norm((X-X_ref))
    minimize sum(error_norm1)

%     minimize sum(error_norm1)/n_data + beta*norm((X-X_ref).*inertia_normal_vec)
%     minimize (X'*(G'*G)*X + 2*b'*G*X + beta*norm((X-X_ref).*inertia_normal_vec))
%     + 2*b.'*G*X  + tau_square_sum
%     minimize norm(G*X+b,2)*norm(G*X+b,2) + tau_square_sum
%     minimize ((G*X+b)'*(G*X+b)-b'*b + tau_square_sum)
    for j = 1:nb
        mj(j) = X((j-1)*10+1);
        hj(:,:,j) = X((j-1)*10+2:(j-1)*10+4);
        Ij(:,:,j) = [X((j-1)*10+5) X((j-1)*10+6) X((j-1)*10+7);
                    X((j-1)*10+6) X((j-1)*10+8) X((j-1)*10+9);
                    X((j-1)*10+7) X((j-1)*10+9) X((j-1)*10+10)];
       
        inertial_sdp_matrix(4,4,j) = X((j-1)*10+1);
        inertial_sdp_matrix(1:3,1:3,j) = 0.5*trace(Ij(:,:,j))*eye(3) - Ij(:,:,j);
        inertial_sdp_matrix(4,1:3,j) =  X((j-1)*10+2:(j-1)*10+4)';
        inertial_sdp_matrix(1:3,4,j) =  X((j-1)*10+2:(j-1)*10+4);
    end
    
    C1 = inertial_sdp_matrix(:,:,1);
    C2 = inertial_sdp_matrix(:,:,2);
    C3 = inertial_sdp_matrix(:,:,3);
    C4 = inertial_sdp_matrix(:,:,4);
    C5 = inertial_sdp_matrix(:,:,5);
    subject to

        C1 == semidefinite(4);
        C2 == semidefinite(4);
        C3 == semidefinite(4);
        C4 == semidefinite(4);
        C5 == semidefinite(4);%%此处约束未适用于任意自由度的urdf模型  需要修改

cvx_end

tau_est = zeros(nb,n_data);
for i = 1:1:n_data
    tau_est(:,i) = K(:,:,i)*X + Bc.*sign(joint_vel(:,i)) + B.*joint_vel(:,i);
end

for i =1:nb
    figure (i)
    T = t:t:t*n_data;
    plot(T,data_tau(i,1:n_data),'-.b')
    hold on
    plot(T,tau_est(i,1:n_data),'r')
    hold on
%     plot(T,0.01*data_qdd(i,1:n_data),'--g')
    xlabel('t(s) 蓝色为测量值 红色为估计值' );
    ylabel('joint torque(Nm)')
end


